#include "lc_a6_motor.h"
#include "ui_lc_a6_motor.h"
#include <QThread>
#include "DataParser.h"
#include "FloraLogger.h"
LC_A6_Motor::LC_A6_Motor(uint8_t id, int32_t max, int32_t min, QWidget *parent) :m_id(id),
    QWidget(parent),
    ui(new Ui::LC_A6_Motor)
{
    ui->setupUi(this);
    max_pos = max;
    min_pos = min;
    zeroOffect = 0;
}

LC_A6_Motor::~LC_A6_Motor()
{
    delete ui;
}

void LC_A6_Motor::setName(QString name)
{
    m_name = name;
    ui->lbName->setText(m_name);
}

void LC_A6_Motor::emitAskPos()
{
    auto temp  = canopen_sdo_read(m_id,PUU_ADDR,0);
    emit sendMessage(temp);
}

void LC_A6_Motor::enabelMotor()
{
    auto temp = canopen_sdo_write(m_id,CONTORL_MODE_ADDR,0,1,WRITE_BYTE_1);
    emit sendMessage(temp);
    temp = canopen_sdo_write(m_id,CONTORL_STATUS_ADDR,0,6,WRITE_BYTE_2);
    emit sendMessage(temp);
    QThread::msleep(20);
    temp = canopen_sdo_write(m_id,CONTORL_STATUS_ADDR,0,7,WRITE_BYTE_2);
    emit sendMessage(temp);
    QThread::msleep(20);
    temp = canopen_sdo_write(m_id,CONTORL_STATUS_ADDR,0,15,WRITE_BYTE_2);
    emit sendMessage(temp);
    QThread::msleep(20);
    temp = canopen_sdo_write(m_id,POR_SPEED_ADDR,0,m_speed,WRITE_BYTE_4);
    emit sendMessage(temp);
    temp = canopen_sdo_write(m_id,POR_ACC_ADDR,0,m_acc_speed,WRITE_BYTE_4);
    emit sendMessage(temp);
    temp = canopen_sdo_write(m_id,PRO_DEC_ADDR,0,m_dec_speed,WRITE_BYTE_4);
    emit sendMessage(temp);
}

void LC_A6_Motor::setTarget(int32_t pod)
{
    pod = zeroOffect + pod;
    if(max_pos !=0 || min_pos !=0)
    {
        if(pod > max_pos)
        {
            pod = max_pos;
        }
        if(pod < min_pos)
        {
            pod = min_pos;
        }
    }
    LOG_DEBUG<<"control_pos"<<pod;
    auto  temp = canopen_sdo_write(m_id,TAR_POS_ADDR,0,pod,WRITE_BYTE_4);
    emit sendMessage(temp);
    temp = canopen_sdo_write(m_id,POR_SPEED_ADDR,0,m_speed,WRITE_BYTE_4);
    emit sendMessage(temp);
    temp = canopen_sdo_write(m_id,POR_ACC_ADDR,0,m_acc_speed,WRITE_BYTE_4);
    emit sendMessage(temp);
    temp = canopen_sdo_write(m_id,PRO_DEC_ADDR,0,m_dec_speed,WRITE_BYTE_4);
    emit sendMessage(temp);
    temp = canopen_sdo_write(m_id,CONTORL_STATUS_ADDR,0,15,WRITE_BYTE_2);
    emit sendMessage(temp);
    temp = canopen_sdo_write(m_id,CONTORL_STATUS_ADDR,0,0x3f,WRITE_BYTE_2);
    emit sendMessage(temp);
    targetPos = pod;
    updateTarPos(targetPos);
}

void LC_A6_Motor::setTarget_2(int32_t pod)
{

    if(max_pos !=0 || min_pos !=0)
    {
        if(pod > max_pos)
        {
            pod = max_pos;
        }
        if(pod < min_pos)
        {
            pod = min_pos;
        }
    }
    LOG_DEBUG<<"control_pos"<<pod;
    auto  temp = canopen_sdo_write(m_id,TAR_POS_ADDR,0,pod,WRITE_BYTE_4);
    emit sendMessage(temp);
    temp = canopen_sdo_write(m_id,POR_SPEED_ADDR,0,m_speed,WRITE_BYTE_4);
    emit sendMessage(temp);
    temp = canopen_sdo_write(m_id,POR_ACC_ADDR,0,m_acc_speed,WRITE_BYTE_4);
    emit sendMessage(temp);
    temp = canopen_sdo_write(m_id,PRO_DEC_ADDR,0,m_dec_speed,WRITE_BYTE_4);
    emit sendMessage(temp);
    temp = canopen_sdo_write(m_id,CONTORL_STATUS_ADDR,0,15,WRITE_BYTE_2);
    emit sendMessage(temp);
    temp = canopen_sdo_write(m_id,CONTORL_STATUS_ADDR,0,0x3f,WRITE_BYTE_2);
    emit sendMessage(temp);
    targetPos = pod;
    updateTarPos(targetPos);
}

void LC_A6_Motor::reciveMessage(struct can_frame fam)
{
    uint8_t funCode = fam.data[0];
    uint16_t addr = fam.data[1] | (fam.data[2]<<8);
    int32_t data_32;
    switch(addr)
    {
        case PUU_ADDR:
            data_32 = parse_int32(fam.data[4],fam.data[5],fam.data[6],fam.data[7]);
            ui->lbCurPos->setNum(data_32);


            curPos = data_32;
            if(m_id == 2 || m_id == 3)
            {
                upDateYX();
            }
            if(m_id == 4 || m_id == 5)
            {
                upDateRoll();
            }
            if(m_id == 6)
            {
                upDateYaw();
            }
            mes_id++;
            updateMesId(mes_id);
        break;
    }
}

void LC_A6_Motor::baseCurPoseAdd(float pos)
{
    int tempos = pos * roll_scale;
    LOG_DEBUG<<"add pos "<<tempos;
    setTarget(curPos + tempos);
}

void LC_A6_Motor::baseCurPoseDown(float pos)
{
    int tempos = pos * roll_scale;
    LOG_DEBUG<<"add pos "<<tempos;
    setTarget(curPos - tempos);
}

void LC_A6_Motor::setYawAngle(float angle)
{
    int temppos = angle * 291271.0;

    setTarget( temppos );
}

void LC_A6_Motor::setY_Trans(float len)
{
    int temppos = len * 524288.0;
    setTarget( temppos );
}

void LC_A6_Motor::setX_Trans(float len)
{
    int temppos = len * 524288.0;
    setTarget( temppos );
}

void LC_A6_Motor::updateMesId(uint32_t dd)
{
    ui->lbId->setText(QString("D%1").arg(dd));
}

void LC_A6_Motor::updateTarPos(int32_t pos)
{
    ui->leTagetPos->setText(QString::number(pos));
}

void LC_A6_Motor::setZeroPos(int32_t pos)
{
    zeroOffect = pos;
    ui->lbZero->setText(QString("zero:%1").arg(pos));
}

void LC_A6_Motor::upDateYX()
{
    float cur  = (curPos - zeroOffect) / x_scale;
    ui->lb_xy->setText(QString("位置:%1").arg(cur,2,'f',3));
    m_xy = cur;
}

void LC_A6_Motor::upDateYaw()
{
    float cur  = (curPos - zeroOffect) / Y_scale;
    ui->lb_xy->setText(QString("角度:%1").arg(cur,3,'f',3));
    m_yaw = cur;
}

float LC_A6_Motor::getYaw()
{
    return m_yaw;
}

void LC_A6_Motor::upDateRoll()
{
    float cur  = (curPos - zeroOffect) / roll_scale;
    ui->lb_xy->setText(QString("角度:%1").arg(cur,3,'f',3));
    m_roll = cur;
}

bool LC_A6_Motor::isArrive()
{
    int error = (curPos - targetPos);
    if(error <20)
    {
        return true;
    }
    else
    {
        return false;
    }
}

void LC_A6_Motor::stop()
{
    auto temp = canopen_sdo_write(m_id,CONTORL_STATUS_ADDR,0,0x002,WRITE_BYTE_2);
    emit sendMessage(temp);
}

void LC_A6_Motor::on_btnAskPos_clicked()
{
    emitAskPos();
}

void LC_A6_Motor::on_btnEnable_clicked()
{
    enabelMotor();
}

void LC_A6_Motor::on_btnSetPos_clicked()
{
    setTarget_2(ui->leTagetPos->text().toInt());
}

void LC_A6_Motor::on_btn_zero_clicked()
{
    if(zeroOffect != 0)
    {
        LOG_DEBUG<<"zero set";
       setTarget(0);
    }

}

void LC_A6_Motor::on_btn_stop_clicked()
{
    stop();
}
